//*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"
#include <limits>

using namespace cv;
using namespace std;

static inline Point2f applyHomography(const Mat_<double>& H, const Point2f& pt) {
    double z = H(2, 0) * pt.x + H(2, 1) * pt.y + H(2, 2);
    if (z) {
        double w = 1. / z;
        return Point2f((float)((H(0, 0) * pt.x + H(0, 1) * pt.y + H(0, 2)) * w), (float)((H(1, 0) * pt.x + H(1, 1) * pt.y + H(1, 2)) * w));
    }
    return Point2f(numeric_limits<float>::max(), numeric_limits<float>::max());
}

static inline void linearizeHomographyAt(const Mat_<double>& H, const Point2f& pt, Mat_<double>& A) {
    A.create(2, 2);
    double p1 = H(0, 0) * pt.x + H(0, 1) * pt.y + H(0, 2),
           p2 = H(1, 0) * pt.x + H(1, 1) * pt.y + H(1, 2),
           p3 = H(2, 0) * pt.x + H(2, 1) * pt.y + H(2, 2),
           p3_2 = p3 * p3;
    if (p3) {
        A(0, 0) = H(0, 0) / p3 - p1 * H(2, 0) / p3_2; // fxdx
        A(0, 1) = H(0, 1) / p3 - p1 * H(2, 1) / p3_2; // fxdy

        A(1, 0) = H(1, 0) / p3 - p2 * H(2, 0) / p3_2; // fydx
        A(1, 1) = H(1, 1) / p3 - p2 * H(2, 1) / p3_2; // fydx
    } else {
        A.setTo(Scalar::all(numeric_limits<double>::max()));
    }
}

class EllipticKeyPoint {
public:
    EllipticKeyPoint();
    EllipticKeyPoint(const Point2f& _center, const Scalar& _ellipse);

    static void convert(const vector<KeyPoint>& src, vector<EllipticKeyPoint>& dst);
    static void convert(const vector<EllipticKeyPoint>& src, vector<KeyPoint>& dst);

    static Mat_<double> getSecondMomentsMatrix(const Scalar& _ellipse);
    Mat_<double> getSecondMomentsMatrix() const;

    void calcProjection(const Mat_<double>& H, EllipticKeyPoint& projection) const;
    static void calcProjection(const vector<EllipticKeyPoint>& src, const Mat_<double>& H, vector<EllipticKeyPoint>& dst);

    Point2f center;
    Scalar ellipse; // 3 elements a, b, c: ax^2+2bxy+cy^2=1
    Size_<float> axes; // half lenght of elipse axes
    Size_<float> boundingBox; // half sizes of bounding box which sides are parallel to the coordinate axes
};

EllipticKeyPoint::EllipticKeyPoint() {
    *this = EllipticKeyPoint(Point2f(0, 0), Scalar(1, 0, 1));
}

EllipticKeyPoint::EllipticKeyPoint(const Point2f& _center, const Scalar& _ellipse) {
    center = _center;
    ellipse = _ellipse;

    Mat_<double> M = getSecondMomentsMatrix(_ellipse), eval;
    eigen(M, eval);
    assert(eval.rows == 2 && eval.cols == 1);
    axes.width = 1.f / (float)sqrt(eval(0, 0));
    axes.height = 1.f / (float)sqrt(eval(1, 0));

    double ac_b2 = ellipse[0] * ellipse[2] - ellipse[1] * ellipse[1];
    boundingBox.width = (float)sqrt(ellipse[2] / ac_b2);
    boundingBox.height = (float)sqrt(ellipse[0] / ac_b2);
}

Mat_<double> EllipticKeyPoint::getSecondMomentsMatrix(const Scalar& _ellipse) {
    Mat_<double> M(2, 2);
    M(0, 0) = _ellipse[0];
    M(1, 0) = M(0, 1) = _ellipse[1];
    M(1, 1) = _ellipse[2];
    return M;
}

Mat_<double> EllipticKeyPoint::getSecondMomentsMatrix() const {
    return getSecondMomentsMatrix(ellipse);
}

void EllipticKeyPoint::calcProjection(const Mat_<double>& H, EllipticKeyPoint& projection) const {
    Point2f dstCenter = applyHomography(H, center);

    Mat_<double> invM; invert(getSecondMomentsMatrix(), invM);
    Mat_<double> Aff; linearizeHomographyAt(H, center, Aff);
    Mat_<double> dstM; invert(Aff * invM * Aff.t(), dstM);

    projection = EllipticKeyPoint(dstCenter, Scalar(dstM(0, 0), dstM(0, 1), dstM(1, 1)));
}

void EllipticKeyPoint::convert(const vector<KeyPoint>& src, vector<EllipticKeyPoint>& dst) {
    if (!src.empty()) {
        dst.resize(src.size());
        for (size_t i = 0; i < src.size(); i++) {
            float rad = src[i].size / 2;
            assert(rad);
            float fac = 1.f / (rad * rad);
            dst[i] = EllipticKeyPoint(src[i].pt, Scalar(fac, 0, fac));
        }
    }
}

void EllipticKeyPoint::convert(const vector<EllipticKeyPoint>& src, vector<KeyPoint>& dst) {
    if (!src.empty()) {
        dst.resize(src.size());
        for (size_t i = 0; i < src.size(); i++) {
            Size_<float> axes = src[i].axes;
            float rad = sqrt(axes.height * axes.width);
            dst[i] = KeyPoint(src[i].center, 2 * rad);
        }
    }
}

void EllipticKeyPoint::calcProjection(const vector<EllipticKeyPoint>& src, const Mat_<double>& H, vector<EllipticKeyPoint>& dst) {
    if (!src.empty()) {
        assert(!H.empty() && H.cols == 3 && H.rows == 3);
        dst.resize(src.size());
        vector<EllipticKeyPoint>::const_iterator srcIt = src.begin();
        vector<EllipticKeyPoint>::iterator       dstIt = dst.begin();
        for (; srcIt != src.end(); ++srcIt, ++dstIt) {
            srcIt->calcProjection(H, *dstIt);
        }
    }
}

static void filterEllipticKeyPointsByImageSize(vector<EllipticKeyPoint>& keypoints, const Size& imgSize) {
    if (!keypoints.empty()) {
        vector<EllipticKeyPoint> filtered;
        filtered.reserve(keypoints.size());
        vector<EllipticKeyPoint>::const_iterator it = keypoints.begin();
        for (int i = 0; it != keypoints.end(); ++it, i++) {
            if (it->center.x + it->boundingBox.width < imgSize.width &&
                    it->center.x - it->boundingBox.width > 0 &&
                    it->center.y + it->boundingBox.height < imgSize.height &&
                    it->center.y - it->boundingBox.height > 0) {
                filtered.push_back(*it);
            }
        }
        keypoints.assign(filtered.begin(), filtered.end());
    }
}

static void overlap(const vector<EllipticKeyPoint>& keypoints1, const vector<EllipticKeyPoint>& keypoints2t, bool commonPart,
                    SparseMat_<float>& overlaps) {
    overlaps.clear();
    if (keypoints1.empty() || keypoints2t.empty()) {
        return;
    }

    int size[] = { keypoints1.size(), keypoints2t.size() };
    overlaps.create(2, size);

    for (size_t i1 = 0; i1 < keypoints1.size(); i1++) {
        EllipticKeyPoint kp1 = keypoints1[i1];
        float maxDist = sqrt(kp1.axes.width * kp1.axes.height),
              fac = 30.f / maxDist;
        if (!commonPart) {
            fac = 3;
        }

        maxDist = maxDist * 4;
        fac = 1.f / (fac * fac);

        EllipticKeyPoint keypoint1a = EllipticKeyPoint(kp1.center, Scalar(fac * kp1.ellipse[0], fac * kp1.ellipse[1], fac * kp1.ellipse[2]));

        for (size_t i2 = 0; i2 < keypoints2t.size(); i2++) {
            EllipticKeyPoint kp2 = keypoints2t[i2];
            Point2f diff = kp2.center - kp1.center;

            if (norm(diff) < maxDist) {
                EllipticKeyPoint keypoint2a = EllipticKeyPoint(kp2.center, Scalar(fac * kp2.ellipse[0], fac * kp2.ellipse[1], fac * kp2.ellipse[2]));
                //find the largest eigenvalue
                float maxx =  ceil((keypoint1a.boundingBox.width > (diff.x + keypoint2a.boundingBox.width)) ?
                                   keypoint1a.boundingBox.width : (diff.x + keypoint2a.boundingBox.width));
                float minx = floor((-keypoint1a.boundingBox.width < (diff.x - keypoint2a.boundingBox.width)) ?
                                   -keypoint1a.boundingBox.width : (diff.x - keypoint2a.boundingBox.width));

                float maxy =  ceil((keypoint1a.boundingBox.height > (diff.y + keypoint2a.boundingBox.height)) ?
                                   keypoint1a.boundingBox.height : (diff.y + keypoint2a.boundingBox.height));
                float miny = floor((-keypoint1a.boundingBox.height < (diff.y - keypoint2a.boundingBox.height)) ?
                                   -keypoint1a.boundingBox.height : (diff.y - keypoint2a.boundingBox.height));
                float mina = (maxx - minx) < (maxy - miny) ? (maxx - minx) : (maxy - miny) ;
                float dr = mina / 50.f;
                float bua = 0.f, bna = 0.f;
                //compute the area
                for (float rx1 = minx; rx1 <= maxx; rx1 += dr) {
                    float rx2 = rx1 - diff.x;
                    for (float ry1 = miny; ry1 <= maxy; ry1 += dr) {
                        float ry2 = ry1 - diff.y;
                        //compute the distance from the ellipse center
                        float e1 = (float)(keypoint1a.ellipse[0] * rx1 * rx1 + 2 * keypoint1a.ellipse[1] * rx1 * ry1 + keypoint1a.ellipse[2] * ry1 * ry1);
                        float e2 = (float)(keypoint2a.ellipse[0] * rx2 * rx2 + 2 * keypoint2a.ellipse[1] * rx2 * ry2 + keypoint2a.ellipse[2] * ry2 * ry2);
                        //compute the area
                        if (e1 < 1 && e2 < 1) { bna++; }
                        if (e1 < 1 || e2 < 1) { bua++; }
                    }
                }
                if (bna > 0) {
                    overlaps.ref(i1, i2) = bna / bua;
                }
            }
        }
    }
}

static void calculateRepeatability(const Mat& img1, const Mat& img2, const Mat& H1to2,
                                   const vector<KeyPoint>& _keypoints1, const vector<KeyPoint>& _keypoints2,
                                   float& repeatability, int& correspondencesCount,
                                   SparseMat_<uchar>* thresholdedOverlapMask = 0) {
    vector<EllipticKeyPoint> keypoints1, keypoints2, keypoints1t, keypoints2t;
    EllipticKeyPoint::convert(_keypoints1, keypoints1);
    EllipticKeyPoint::convert(_keypoints2, keypoints2);

    // calculate projections of key points
    EllipticKeyPoint::calcProjection(keypoints1, H1to2, keypoints1t);
    Mat H2to1; invert(H1to2, H2to1);
    EllipticKeyPoint::calcProjection(keypoints2, H2to1, keypoints2t);

    bool ifEvaluateDetectors = !thresholdedOverlapMask; // == commonPart
    float overlapThreshold;
    if (ifEvaluateDetectors) {
        overlapThreshold = 1.f - 0.4f;

        // remove key points from outside of the common image part
        Size sz1 = img1.size(), sz2 = img2.size();
        filterEllipticKeyPointsByImageSize(keypoints1, sz1);
        filterEllipticKeyPointsByImageSize(keypoints1t, sz2);
        filterEllipticKeyPointsByImageSize(keypoints2, sz2);
        filterEllipticKeyPointsByImageSize(keypoints2t, sz1);
    } else {
        overlapThreshold = 1.f - 0.5f;
    }
    int minCount = min(keypoints1.size(), keypoints2t.size());

    // calculate overlap errors
    SparseMat_<float> overlaps;
    overlap(keypoints1, keypoints2t, ifEvaluateDetectors, overlaps);

    correspondencesCount = -1;
    repeatability = -1.f;
    const int* size = overlaps.size();
    if (!size || overlaps.nzcount() == 0) {
        return;
    }

    if (ifEvaluateDetectors) {
        // threshold the overlaps
        for (int y = 0; y < size[0]; y++) {
            for (int x = 0; x < size[1]; x++) {
                if (overlaps(y, x) < overlapThreshold) {
                    overlaps.erase(y, x);
                }
            }
        }

        // regions one-to-one matching
        correspondencesCount = 0;
        while (overlaps.nzcount() > 0) {
            double maxOverlap = 0;
            int maxIdx[2];
            minMaxLoc(overlaps, 0, &maxOverlap, 0, maxIdx);
            for (size_t i1 = 0; i1 < keypoints1.size(); i1++) {
                overlaps.erase(i1, maxIdx[1]);
            }
            for (size_t i2 = 0; i2 < keypoints2t.size(); i2++) {
                overlaps.erase(maxIdx[0], i2);
            }
            correspondencesCount++;
        }
        repeatability = minCount ? (float)correspondencesCount / minCount : -1;
    } else {
        thresholdedOverlapMask->create(2, size);
        for (int y = 0; y < size[0]; y++) {
            for (int x = 0; x < size[1]; x++) {
                float val = overlaps(y, x);
                if (val >= overlapThreshold) {
                    thresholdedOverlapMask->ref(y, x) = 1;
                }
            }
        }
    }
}

void cv::evaluateFeatureDetector(const Mat& img1, const Mat& img2, const Mat& H1to2,
                                 vector<KeyPoint>* _keypoints1, vector<KeyPoint>* _keypoints2,
                                 float& repeatability, int& correspCount,
                                 const Ptr<FeatureDetector>& _fdetector) {
    Ptr<FeatureDetector> fdetector(_fdetector);
    vector<KeyPoint> *keypoints1, *keypoints2, buf1, buf2;
    keypoints1 = _keypoints1 != 0 ? _keypoints1 : &buf1;
    keypoints2 = _keypoints2 != 0 ? _keypoints2 : &buf2;

    if ((keypoints1->empty() || keypoints2->empty()) && fdetector.empty()) {
        CV_Error(CV_StsBadArg, "fdetector must be no empty when keypoints1 or keypoints2 is empty");
    }

    if (keypoints1->empty()) {
        fdetector->detect(img1, *keypoints1);
    }
    if (keypoints2->empty()) {
        fdetector->detect(img1, *keypoints2);
    }

    calculateRepeatability(img1, img2, H1to2, *keypoints1, *keypoints2, repeatability, correspCount);
}

struct DMatchForEvaluation : public DMatch {
    uchar isCorrect;
    DMatchForEvaluation(const DMatch& dm) : DMatch(dm) {}
};

static inline float recall(int correctMatchCount, int correspondenceCount) {
    return correspondenceCount ? (float)correctMatchCount / (float)correspondenceCount : -1;
}

static inline float precision(int correctMatchCount, int falseMatchCount) {
    return correctMatchCount + falseMatchCount ? (float)correctMatchCount / (float)(correctMatchCount + falseMatchCount) : -1;
}

void cv::computeRecallPrecisionCurve(const vector<vector<DMatch> >& matches1to2,
                                     const vector<vector<uchar> >& correctMatches1to2Mask,
                                     vector<Point2f>& recallPrecisionCurve) {
    CV_Assert(matches1to2.size() == correctMatches1to2Mask.size());

    vector<DMatchForEvaluation> allMatches;
    int correspondenceCount = 0;
    for (size_t i = 0; i < matches1to2.size(); i++) {
        for (size_t j = 0; j < matches1to2[i].size(); j++) {
            DMatchForEvaluation match = matches1to2[i][j];
            match.isCorrect = correctMatches1to2Mask[i][j] ;
            allMatches.push_back(match);
            correspondenceCount += match.isCorrect != 0 ? 1 : 0;
        }
    }

    std::sort(allMatches.begin(), allMatches.end());

    int correctMatchCount = 0, falseMatchCount = 0;
    recallPrecisionCurve.resize(allMatches.size());
    for (size_t i = 0; i < allMatches.size(); i++) {
        if (allMatches[i].isCorrect) {
            correctMatchCount++;
        } else {
            falseMatchCount++;
        }

        float r = recall(correctMatchCount, correspondenceCount);
        float p =  precision(correctMatchCount, falseMatchCount);
        recallPrecisionCurve[i] = Point2f(1 - p, r);
    }
}

float cv::getRecall(const vector<Point2f>& recallPrecisionCurve, float l_precision) {
    float recall = -1;

    if (l_precision >= 0 && l_precision <= 1) {
        int bestIdx = -1;
        float minDiff = FLT_MAX;
        for (size_t i = 0; i < recallPrecisionCurve.size(); i++) {
            float curDiff = std::fabs(l_precision - recallPrecisionCurve[i].x);
            if (curDiff <= minDiff) {
                bestIdx = i;
                minDiff = curDiff;
            }
        }

        recall = recallPrecisionCurve[bestIdx].y;
    }

    return recall;
}

void cv::evaluateDescriptorMatch(const Mat& img1, const Mat& img2, const Mat& H1to2,
                                 vector<KeyPoint>& keypoints1, vector<KeyPoint>& keypoints2,
                                 vector<vector<DMatch> >* _matches1to2, vector<vector<uchar> >* _correctMatches1to2Mask,
                                 vector<Point2f>& recallPrecisionCurve,
                                 const Ptr<GenericDescriptorMatch>& _dmatch) {
    Ptr<GenericDescriptorMatch> dmatch = _dmatch;
    dmatch->clear();

    vector<vector<DMatch> > *matches1to2, buf1;
    vector<vector<uchar> > *correctMatches1to2Mask, buf2;
    matches1to2 = _matches1to2 != 0 ? _matches1to2 : &buf1;
    correctMatches1to2Mask = _correctMatches1to2Mask != 0 ? _correctMatches1to2Mask : &buf2;

    if (keypoints1.empty()) {
        CV_Error(CV_StsBadArg, "keypoints1 must be no empty");
    }

    if (matches1to2->empty() && dmatch.empty()) {
        CV_Error(CV_StsBadArg, "dmatch must be no empty when matches1to2 is empty");
    }

    bool computeKeypoints2ByPrj = keypoints2.empty();
    if (computeKeypoints2ByPrj) {
        assert(0);
        // TODO: add computing keypoints2 from keypoints1 using H1to2
    }

    if (matches1to2->empty() || computeKeypoints2ByPrj) {
        dmatch->clear();
        dmatch->add(img2, keypoints2);
        // TODO: use more sophisticated strategy to choose threshold
        dmatch->match(img1, keypoints1, *matches1to2, std::numeric_limits<float>::max());
    }
    float repeatability;
    int correspCount;
    SparseMat_<uchar> thresholdedOverlapMask; // thresholded allOverlapErrors
    calculateRepeatability(img1, img2, H1to2,
                           keypoints1, keypoints2,
                           repeatability, correspCount,
                           &thresholdedOverlapMask);

    correctMatches1to2Mask->resize(matches1to2->size());
    int ddd = 0;
    for (size_t i = 0; i < matches1to2->size(); i++) {
        (*correctMatches1to2Mask)[i].resize((*matches1to2)[i].size());
        for (size_t j = 0; j < (*matches1to2)[i].size(); j++) {
            int indexQuery = (*matches1to2)[i][j].indexQuery;
            int indexTrain = (*matches1to2)[i][j].indexTrain;
            (*correctMatches1to2Mask)[i][j] = thresholdedOverlapMask(indexQuery, indexTrain);
            ddd += thresholdedOverlapMask(indexQuery, indexTrain) != 0 ? 1 : 0;
        }
    }

    computeRecallPrecisionCurve(*matches1to2, *correctMatches1to2Mask, recallPrecisionCurve);
}
